#include "control.h"

float Turn_P = 1.8, Turn_D = 0.0;
float Speed_P = 30, Speed_D = 0.0;

float Turn_error=0.0;
float Turn_error_last=0.0;
float Speed_error=0.0;
float Speed_error_last=0.0;

float Turn_Out=0;
float Motor_Out=0;

void Turn_Control(void)
{
	Turn_error = -XY_Deviation;
	Turn_Out = Turn_error*Turn_P+(Turn_error-Turn_error_last)*Turn_D;
	Turn_error_last = Turn_error;
}

void Speed_Control(void)
{
	Speed_error = XY_Deviation;
	Motor_Out = Speed_error*Speed_P+(Speed_error-Speed_error_last)*Speed_D;
	Speed_error_last = Speed_error;
}



